/* 
 * File:   AttractorBehav.cpp
 * Author: fab
 * 
 * Created on 1 febbraio 2013, 11.34
 */

#include "AttractorBehav.h"

AttractorBehav::AttractorBehav() {
}

AttractorBehav::AttractorBehav(const AttractorBehav& orig) {
}

AttractorBehav::~AttractorBehav() {
}

geometry_msgs::Twist AttractorBehav::runToXY(float x, float y){
  geometry_msgs::Twist cmd;
  cmd.linear.x=0;
  cmd.linear.y=0;
  cmd.linear.z=0;
  cmd.angular.x=0;
  cmd.angular.y=0;
  
  //std::cout<<"atan2: "<<atan2(y,x)<<"  alpha:"<<atan2(y,x)*180/M_PI<<std::endl;
  
  cmd.angular.z=sin(atan2(y,x));
  
  return cmd;
}

geometry_msgs::Twist AttractorBehav::runToAlpha(float a){
  geometry_msgs::Twist cmd;
  cmd.linear.x=0;
  cmd.linear.y=0;
  cmd.linear.z=0;
  cmd.angular.x=0;
  cmd.angular.y=0;
  
  //std::cout<<"atan2: "<<atan2(y,x)<<"  alpha:"<<atan2(y,x)*180/M_PI<<std::endl;
  
  cmd.angular.z=sin(a);
  
  return cmd;
}
